#include "xtypes.h"
#include "xaux.h"
#include "xmath.h"
#include "util_vector.h"
#include "util_matrix.h"

CMatrix4x4 fnZerosMatrix()
{
	CMatrix4x4 mt;
	mt.LoadZeros();
	return mt;
}


CMatrix4x4 fnIdentityMatrix()
{
	CMatrix4x4 mt;
	mt.LoadIdentity();
	return mt;
}


CMatrix4x4 fnTranslateMatrix(float_t dx, float_t dy, float_t dz)
{
	CMatrix4x4 mt;
	mt = fnIdentityMatrix();
	mt.fnMatrix(3,0) = dx;
	mt.fnMatrix(3,1) = dy;
	mt.fnMatrix(3,2) = dz;

	return mt;
}


CMatrix4x4 fnScaleMatrix(float_t sx, float_t sy, float_t sz)
{
	CMatrix4x4 mt;
	mt = fnIdentityMatrix();
	mt.fnMatrix(0,0) = sx;
	mt.fnMatrix(1,1) = sy;
	mt.fnMatrix(2,2) = sz;

	return mt;
}


CMatrix4x4 fnXRotateMatrix(float_t angle)
{
	CMatrix4x4 mt;
	mt = fnIdentityMatrix();
	mt.fnMatrix(1,1) = xCos(angle);
	mt.fnMatrix(1,2) = xSin(angle);
	mt.fnMatrix(2,1) = -xSin(angle);
	mt.fnMatrix(2,2) = xCos(angle);

	return mt;
}


CMatrix4x4 fnYRotateMatrix(float_t angle)
{
	CMatrix4x4 mt;
	mt = fnIdentityMatrix();
	mt.fnMatrix(0,0) = xCos(angle);
	mt.fnMatrix(0,2) = -xSin(angle);
	mt.fnMatrix(2,0) = xSin(angle);
	mt.fnMatrix(2,2) = xCos(angle);

	return mt;
}


CMatrix4x4 fnZRotateMatrix(float_t angle)

{
	CMatrix4x4 mt;
	mt = fnIdentityMatrix();
	mt.fnMatrix(0,0) = xCos(angle);
	mt.fnMatrix(0,1) = xSin(angle);
	mt.fnMatrix(1,0) = -xSin(angle);
	mt.fnMatrix(1,1) = xCos(angle);

	return mt;
}


CMatrix4x4 fnRotateMatrix(float_t angle, point3_t pt_start, vector3_t axis_dir)
{
	float_t fsin, fcos;
	fsin = xSin(angle);
	fcos = xCos(angle);

	axis_dir = xVector3Normalize(axis_dir);


	CMatrix4x4 mt;
	mt = fnIdentityMatrix();

	mt.fnMatrix(0,0) = (1.0 - fcos) * (axis_dir.x * axis_dir.x) + (fcos * 1.0);
	mt.fnMatrix(0,1) = (1.0 - fcos) * (axis_dir.x * axis_dir.y) - (fsin * axis_dir.z);
	mt.fnMatrix(0,2) = (1.0 - fcos) * (axis_dir.x * axis_dir.z) + (fsin * axis_dir.y);
	mt.fnMatrix(0,3) = 0;

	mt.fnMatrix(1,0) = (1.0 - fcos) * (axis_dir.x * axis_dir.y) + (fsin * axis_dir.z);
	mt.fnMatrix(1,1) = (1.0 - fcos) * (axis_dir.y * axis_dir.y) + (fcos * 1.0);
	mt.fnMatrix(1,2) = (1.0 - fcos) * (axis_dir.y * axis_dir.z) - (fsin * axis_dir.x);
	mt.fnMatrix(1,3) = 0;

	mt.fnMatrix(2,0) = (1.0 - fcos) * (axis_dir.x * axis_dir.z) - (fsin * axis_dir.y);
	mt.fnMatrix(2,1) = (1.0 - fcos) * (axis_dir.y * axis_dir.z) + (fsin * axis_dir.x);
	mt.fnMatrix(2,2) = (1.0 - fcos) * (axis_dir.z * axis_dir.z) + (fcos * 1.0);
	mt.fnMatrix(2,3) = 0;

	mt.fnMatrix(3,0) = 0;
	mt.fnMatrix(3,1) = 0;
	mt.fnMatrix(3,2) = 0;
	mt.fnMatrix(3,3) = 1.0;

	mt = fnTranslateMatrix(-pt_start.x, -pt_start.y, -pt_start.z) * mt
		* fnTranslateMatrix(pt_start.x, pt_start.y, pt_start.z);

	return mt;
}


float_t xEuclidDistance(point3_t p3d0, point3_t p3d1)
{
	float_t delta_x = p3d1.x-p3d0.x;
	float_t delta_y = p3d1.y-p3d0.y;
	float_t delta_z = p3d1.z-p3d0.z;

	return xMath_sqrt(delta_x*delta_x + delta_y*delta_y + delta_z*delta_z);
}

vector3_t xEuclidNormal(point3_t p3d0, point3_t p3d1, point3_t p3d2)
{
	vector3_t A,B;
	A = p3d2 - p3d1;
	B = p3d0 - p3d1;

	return xVector3Normalize(A*B);
}




int_t xFactorial(int_t n)
{
	//recursion
	// 	if(n<=0)
	// 	{
	// 		return 1;
	// 	}
	// 	else
	// 	{
	// 		return n*xFactorial(n-1);
	// 	}

	//iteration
	int i, ans=1;
	for(i=1;i<=n;++i)
	{
		ans*=i;
	}
	return ans;

}


//-----------------------------------------------------------------------------
// Name: xLinearMap
// Desc: map 0..1 into low_bound..high_bound
//-----------------------------------------------------------------------------
float_t xLinearMap(float_t low_bound, float_t high_bound, float_t t)
{
	return ((low_bound)+(t)*((high_bound)-(low_bound)));
}


point2_t xBiLinearMap(float_t left, float_t top, float_t right, float_t bottom, float_t u, float_t v)
{
	point2_t p2d;
	p2d.x = xLinearMap(left, right, u);
	p2d.y = xLinearMap(top, bottom, v);

	return p2d;
}

float_t xBernstein(INT i, INT n, float_t t)
{
	return ((xFactorial(n)) / (xFactorial(i)*xFactorial(n-i))) * xMath_pow(t,i) * xMath_pow(1-t, n-i);
}


point3_t xBezierSampling(point3_t* pi, INT n, float_t t)
{
	int i;
	point3_t p;

	p.x = 0;
	p.y = 0;
	p.z = 0;
	for(i=0;i<=n;++i)
	{
		p.x += pi[i].x * xBernstein(i,n,t);
		p.y += pi[i].y * xBernstein(i,n,t);
		p.z += pi[i].z * xBernstein(i,n,t);
	}

	return p;
}


point3_t xSquareBezierSampling(point3_t* pij, INT m, INT n, float_t u, float_t w)
{
	point3_t p;
	int i,j;

	p.x = 0;
	p.y = 0;
	p.z = 0;
	for(i=0;i<=n;++i)
	{
		for(j=0;j<=m;j++)
		{
			p.x += pij[i*(m+1)+j].x * xBernstein(i,n,u) * xBernstein(j,m,w);
			p.y += pij[i*(m+1)+j].y * xBernstein(i,n,u) * xBernstein(j,m,w);
			p.z += pij[i*(m+1)+j].z * xBernstein(i,n,u) * xBernstein(j,m,w);
		}
	}

	return p;
}



point3_t xSphereSampling(float_t r, float_t u, float_t v)
{
	point3_t pt;
	float_t alpha, beta;
	alpha = xLinearMap(0, 2*MATH_PI, v);
	beta = xLinearMap(-MATH_PI/2, MATH_PI/2, u);

	pt.x = r*xCos(beta)*xCos(alpha);
	pt.y = r*xCos(beta)*xSin(alpha);
	pt.z = r*xSin(beta);

	return pt;
}



point3_t xCirqueSampling(float_t rTrack, float_t rSmall, float_t u, float_t v)
{
	point3_t pt;
	float_t alpha, beta;
	alpha = xLinearMap(0, 2*MATH_PI, u);
	beta = xLinearMap(0, 2*MATH_PI, v);

	pt.x = (rTrack+rSmall*xCos(beta))*xCos(alpha);
	pt.y = (rTrack+rSmall*xCos(beta))*xSin(alpha);
	pt.z = rSmall*xSin(beta);

	return pt;
}


